﻿//------------------------------------------------------------------------------
// <copyright file="AvateeringXNA.cs" company="Microsoft">
//     Copyright (c) Microsoft Corporation.  All rights reserved.
// </copyright>
//------------------------------------------------------------------------------

namespace Microsoft.Samples.Kinect.Avateering
{
    using System;
    using System.Diagnostics; 
    using System.Collections.Generic;
    using System.Linq;
    using Microsoft.Kinect;
    using Microsoft.Xna.Framework;
    using Microsoft.Xna.Framework.Graphics;
    using Microsoft.Xna.Framework.Input;

    /// <summary>
    /// Sample game showing how to display skinned character and avateer with Kinect for Windows.
    /// </summary>
    public class AvateeringXNA : Microsoft.Xna.Framework.Game
    {
        #region Fields

        /// <summary>
        /// This is used to adjust the window size. The height is set automatically from the width using a 4:3 ratio.
        /// </summary>
        private const int WindowedWidth = 800;

        private int gameLoopCounter = 0;

        /// <summary>
        /// This is used to adjust the fullscreen window size. Only valid resolutions can be set.
        /// </summary>
        private const int FullScreenWidth = 1280;

        /// <summary>
        /// This is used to adjust the fullscreen window size. Only valid resolutions can be set.
        /// </summary>
        private const int FullScreenHeight = 1024;

        /// <summary>
        /// Camera Arc Increment value.
        /// </summary>
        private const float CameraArcIncrement = 0.1f;

        /// <summary>
        /// Camera Arc angle limit value.
        /// </summary>
        private const float CameraArcAngleLimit = 90.0f;

        /// <summary>
        /// Camera Zoom Increment value.
        /// </summary>
        private const float CameraZoomIncrement = 0.25f;

        /// <summary>
        /// Camera Max Distance value.
        /// </summary>
        private const float CameraMaxDistance = 500.0f;

        /// <summary>
        /// Camera Min Distance value.
        /// </summary>
        private const float CameraMinDistance = 10.0f;

        /// <summary>
        /// Camera starting Distance value.
        /// </summary>
        private const float CameraHeight = 40.0f;

        /// <summary>
        /// Camera starting Distance value.
        /// </summary>
        private const float CameraStartingTranslation = 90.0f;

        /// <summary>
        /// The "Dude" model mesh is defined at an arbitrary size in centimeters.
        /// Here we re-scale the Kinect translation so the model appears to walk more correctly on the ground.
        /// </summary>
        private static readonly Vector3 SkeletonTranslationScaleFactor = new Vector3(40.0f, 40.0f, 40.0f);

        /// <summary>
        /// The graphics device manager provided by XNA.
        /// </summary>
        private readonly GraphicsDeviceManager graphics;

        /// <summary>
        /// This control selects a sensor, and displays a notice if one is
        /// not connected.
        /// </summary>
        private readonly KinectChooser chooser;

        /// <summary>
        /// This manages the rendering of the depth stream.
        /// </summary>
        private DepthStreamRenderer depthStream;

        /// <summary>
        /// This manages the rendering of the skeleton over the depth stream.
        /// </summary>
        private SkeletonStreamRenderer skeletonStream;

        /// <summary>
        /// This is the XNA Basic Effect used in drawing.
        /// </summary>
        private BasicEffect effect;    

        /// <summary>
        /// This is the SpriteBatch used for rendering the header/footer.
        /// </summary>
        private SpriteBatch spriteBatch;

        /// <summary>
        /// This is used when toggling between windowed and fullscreen mode.
        /// </summary>
        private bool fullscreenMode = false;

        /// <summary>
        /// This tracks the previous keyboard state.
        /// </summary>
        private KeyboardState previousKeyboard;

        /// <summary>
        /// This tracks the current keyboard state.
        /// </summary>
        private KeyboardState currentKeyboard;

        /// <summary>
        /// This is the texture for the header.
        /// </summary>
        private Texture2D header;

        /// <summary>
        /// This is the coordinate cross we use to draw the world coordinate system axes.
        /// </summary>
        private CoordinateCross worldAxes;

        /// <summary>
        /// The 3D avatar mesh.
        /// </summary>
        private Model currentModel;

        /// <summary>
        /// Store the mapping between the NuiJoint and the Avatar Bone index.
        /// </summary>
        private Dictionary<JointType, int> nuiJointToAvatarBoneIndex;

        /// <summary>
        /// The 3D avatar mesh animator.
        /// </summary>
        private AvatarAnimator animator;

        /// <summary>
        /// Viewing Camera arc.
        /// </summary>
        private float cameraArc = 0;

        /// <summary>
        /// Viewing Camera current rotation.
        /// The virtual camera starts where Kinect is looking i.e. looking along the Z axis, with +X left, +Y up, +Z forward
        /// for the avatar 180 is facing and zero is back to you
        /// when we added 180 the stick man moves out the field of the camera. 
        /// </summary>
        private float cameraRotation = 0; 

        /// <summary>
        /// Viewing Camera distance from origin.
        /// The "Dude" model is defined in centimeters, hence all the units we use here are cm.
        /// </summary>
        private float cameraDistance = CameraStartingTranslation;

        /// <summary>
        /// Viewing Camera view matrix.
        /// </summary>
        private Matrix view;

        /// <summary>
        /// Viewing Camera projection matrix.
        /// </summary>
        private Matrix projection;
        /// <summary>
        /// Coordinate mapper to map one type of point to another
        /// </summary>
        private CoordinateMapper coordinateMapper = null;

        /// <summary>
        /// Draw the simple planar grid for avatar to stand on if true.
        /// </summary>
        private bool drawGrid;

        /// <summary>
        /// Simple planar grid for avatar to stand on.
        /// </summary>
        private GridXz planarXzGrid;

        /// <summary>
        /// Draw the avatar only when the player skeleton is detected in the depth image.
        /// </summary>
        private bool drawAvatarOnlyWhenPlayerDetected;

        /// <summary>
        /// Flag for first detection of skeleton.
        /// </summary>
        private bool skeletonDetected;

        /// <summary>
        /// Sets a seated posture when Seated Mode is on.
        /// </summary>
        private bool setSeatedPostureInSeatedMode;

        /// <summary>
        /// Fix the avatar hip center draw height.
        /// </summary>
        private bool fixAvatarSpineBaseDrawHeight;

        /// <summary>
        /// Avatar hip center draw height.
        /// </summary>
        private float avatarHipCenterDrawHeight;

        /// <summary>
        /// Adjust Avatar lean when leaning back to reduce lean.
        /// </summary>
        private bool leanAdjust; 

        #endregion

        #region Initialization

        /// <summary>
        /// Initializes a new instance of the AvateeringXNA class.
        /// </summary>
        public AvateeringXNA()
        {
            this.Window.Title = "Avateering";
            this.IsFixedTimeStep = false;
            this.IsMouseVisible = true;

            // Setup the graphics device for rendering
            this.graphics = new GraphicsDeviceManager(this);
            this.SetScreenMode();
            this.graphics.PreparingDeviceSettings += this.GraphicsDevicePreparingDeviceSettings;
            this.graphics.SynchronizeWithVerticalRetrace = true;

            Content.RootDirectory = "Content";

            // The Kinect sensor will use 640x480 for the color stream (default) and 320x240 for depth
            this.chooser = new KinectChooser(this, ColorImageFormat.Rgba, new DepthImageFormat());
            this.Services.AddService(typeof(KinectChooser), this.chooser);

            // Optionally set near mode for close range avateering (0.4m up to 3m)
            this.chooser.NearMode = false;

            // Optionally set seated mode for upper-body only tracking here (typically used with near mode for close to camera tracking)
            this.chooser.SeatedMode = false;

            // Adding these objects as XNA Game components enables automatic calls to the overridden LoadContent, Update, etc.. methods
            this.Components.Add(this.chooser);

            // get the coordinate mapper
            this.coordinateMapper = this.chooser.Sensor.CoordinateMapper;

            // Create a ground plane for the model to stand on
            this.planarXzGrid = new GridXz(this, new Vector3(0, 0, 0), new Vector2(500, 500), new Vector2(10, 10), Color.Black);
            this.Components.Add(this.planarXzGrid);
            this.drawGrid = true;

            this.worldAxes = new CoordinateCross(this, 500);
            this.Components.Add(this.worldAxes);

            // Create the avatar animator
            this.animator = new AvatarAnimator(this, this.RetargetMatrixHierarchyToAvatarMesh, AvateeringXNA.SkeletonTranslationScaleFactor);
            this.Components.Add(this.animator);

            // Drawing options
            this.setSeatedPostureInSeatedMode = true;
            this.drawAvatarOnlyWhenPlayerDetected = true;
            this.skeletonDetected = false;
            this.leanAdjust = true;

            // Here we can force the avatar to be drawn at fixed height in the XNA virtual world.
            // The reason we may use this is because the sensor height above the physical floor
            // and the feet locations are not always known. Hence the avatar cannot be correctly 
            // placed on the ground plane or will be very jumpy.
            // Note: this will prevent the avatar from jumping and crouching.
            this.fixAvatarSpineBaseDrawHeight = true;
            this.avatarHipCenterDrawHeight = 0.8f;  // in meters

            // Setup the depth stream
            this.depthStream = new DepthStreamRenderer(this);

            // Setup the skeleton stream the same as depth stream 
            this.skeletonStream = new SkeletonStreamRenderer(this, this.SkeletonToDepthMap);
            
            // Update Depth and Skeleton Stream size and location based on the back-buffer
            this.UpdateStreamSizeAndLocation();

            this.previousKeyboard = Keyboard.GetState();
        }

        /// <summary>
        /// Gets the KinectChooser from the services.
        /// </summary>
        public KinectChooser Chooser
        {
            get
            {
                return (KinectChooser)this.Services.GetService(typeof(KinectChooser));
            }
        }

        /// <summary>
        /// Override this method to query for any required services, and load any non-graphics resources. 
        /// Use LoadContent to load graphics resources.
        ///Initialize is called before Draw, so the length of time spent executing code in this method 
        ///will be experienced by the user as a delay before he or she sees the initial game screen.
        /// </summary
        protected override void Initialize()
        {
            base.Initialize();
        }

        protected override void Finalize()
        {
            this.chooser.Dispose();
        }

        /// <summary>
        /// Gets the SpriteBatch from the services.
        /// </summary>
        public SpriteBatch SharedSpriteBatch
        {
            get
            {
                return (SpriteBatch)this.Services.GetService(typeof(SpriteBatch));
            }
        }

        /// <summary>
        /// Gets or sets the last frames skeleton data.
        /// </summary>
        private static Body[] SkeletonData { get; set; }

        /// <summary>
        /// Load the graphics content.
        /// </summary>
        protected override void LoadContent()
        {

            // Create the spritebatch to draw the 3D items
            this.spriteBatch = new SpriteBatch(this.GraphicsDevice);
            this.Services.AddService(typeof(SpriteBatch), this.spriteBatch);

            this.header = Content.Load<Texture2D>("Header");

            // Create the XNA Basic Effect for line drawing
            this.effect = new BasicEffect(GraphicsDevice);
            if (null == this.effect)
            {
                throw new InvalidOperationException("Cannot load Basic Effect");
            }

            this.effect.VertexColorEnabled = true;

            // Load the model.
            this.currentModel = Content.Load<Model>("dude");
            if (null == this.currentModel)
            {
                throw new InvalidOperationException("Cannot load 3D avatar model");
            }

            // Add the model to the avatar animator
            this.animator.Avatar = this.currentModel;
            this.animator.AvatarHipCenterHeight = this.avatarHipCenterDrawHeight;

            // Set the Nui joint to model mapping for this avatar
            this.BuildJointHierarchy();

            base.LoadContent();
        }

        /// <summary>
        /// This function configures the mapping between the Nui Skeleton bones/joints and the Avatar bones/joints
        /// </summary>
        protected void BuildJointHierarchy()
        {
            // "Dude.fbx" bone index definitions
            // These are described as the "bone" that the transformation affects.
            // The rotation values are stored at the start joint before the bone (i.e. at the shared joint with the end of the parent bone).
            // 0 = root node
            // 1 = pelvis
            // 2 = spine
            // 3 = spine1
            // 4 = spine2
            // 5 = spine3
            // 6 = neck
            // 7 = head
            // 8-11 = eyes
            // 12 = Left clavicle (joint between spine and shoulder)
            // 13 = Left upper arm (joint at left shoulder)
            // 14 = Left forearm
            // 15 = Left hand
            // 16-30 = Left hand finger bones
            // 31 = Right clavicle (joint between spine and shoulder)
            // 32 = Right upper arm (joint at left shoulder)
            // 33 = Right forearm
            // 34 = Right hand
            // 35-49 = Right hand finger bones
            // 50 = Left Thigh
            // 51 = Left Knee
            // 52 = Left Ankle
            // 53 = Left Ball
            // 54 = Right Thigh
            // 55 = Right Knee
            // 56 = Right Ankle
            // 57 = Right Ball

            // For the Kinect NuiSkeleton, the joint at the end of the bone describes the rotation to get there, 
            // and the root orientation is in HipCenter. This is different to the Avatar skeleton described above.
            if (null == this.nuiJointToAvatarBoneIndex)
            {
                this.nuiJointToAvatarBoneIndex = new Dictionary<JointType, int>();
            }

            // Note: the actual hip center joint in the Avatar mesh has a root node (index 0) as well, which we ignore here for rotation.
            this.nuiJointToAvatarBoneIndex.Add(JointType.SpineBase, 1);
            this.nuiJointToAvatarBoneIndex.Add(JointType.SpineMid, 4);
            this.nuiJointToAvatarBoneIndex.Add(JointType.SpineShoulder, 6);
            this.nuiJointToAvatarBoneIndex.Add(JointType.Head, 7);
            this.nuiJointToAvatarBoneIndex.Add(JointType.ShoulderLeft, 12);
            this.nuiJointToAvatarBoneIndex.Add(JointType.ElbowLeft, 13);
            this.nuiJointToAvatarBoneIndex.Add(JointType.WristLeft, 14);
            this.nuiJointToAvatarBoneIndex.Add(JointType.HandLeft, 15);
            this.nuiJointToAvatarBoneIndex.Add(JointType.ElbowRight, 32);
            this.nuiJointToAvatarBoneIndex.Add(JointType.WristRight, 33);
            this.nuiJointToAvatarBoneIndex.Add(JointType.HandRight, 34);
            this.nuiJointToAvatarBoneIndex.Add(JointType.KneeLeft, 50);
            this.nuiJointToAvatarBoneIndex.Add(JointType.AnkleLeft, 51);
            this.nuiJointToAvatarBoneIndex.Add(JointType.FootLeft, 52);
            this.nuiJointToAvatarBoneIndex.Add(JointType.KneeRight, 54);
            this.nuiJointToAvatarBoneIndex.Add(JointType.AnkleRight, 55);
            this.nuiJointToAvatarBoneIndex.Add(JointType.FootRight, 56);
        }

        #endregion

        #region Update

        /// <summary>
        /// Allows the game to run logic.
        /// </summary>
        /// <param name="gameTime">The gametime.</param>
        protected override void Update(GameTime gameTime)
        {
            gameLoopCounter++;
            if (gameLoopCounter % 999 == 0)
                Debug.WriteLine("gameloopcounter=" + gameLoopCounter);
            // Update saved state.
            this.previousKeyboard = this.currentKeyboard;

            // If the sensor is not found, not running, or not connected, stop now
            if (null == this.chooser || null == this.chooser.Sensor || false == this.chooser.Sensor.IsOpen)
            {
                Debug.WriteLineIf(gameLoopCounter % 5 == 0, "holy cow the Kinect is not HAPPY!");
                return;
            }
            else
            {
                if (!this.chooser.Sensor.BodyFrameSource.IsActive)
                    this.chooser.openReader();
            }

            bool newFrame = false;

                    // Select the first tracked skeleton we see to avateer
                    Body rawSkeleton = this.chooser.getReaderActiveBody();

                    if (null != this.animator)
                    {
                        if (null != rawSkeleton)
                        {
                            this.animator.CopySkeleton(rawSkeleton);
                            this.animator.FloorClipPlane = this.chooser.getFloorClipPlane();

                            // Reset the filters if the skeleton was not seen before now
                            if (this.skeletonDetected == false)
                            { 
                                this.animator.Reset();
                            }

                            this.skeletonDetected = true;
                            this.animator.SkeletonVisible = true;
                            newFrame = true;
                        }
                        else
                        {
                            this.skeletonDetected = false;
                            this.animator.SkeletonVisible = false;
                        }
                    }

                // Call the stream update manually as they are not a game component
                if (null != this.depthStream && null != this.skeletonStream)
                {
                    this.depthStream.Update(gameTime);
                    this.skeletonStream.Update(gameTime, SkeletonData);
                }
                else
                {
                    Debug.WriteLine("this.depthStream && null=" + this.depthStream + " != this.skeletonStream=" + this.skeletonStream);
                    if (null == this.depthStream)
                        this.depthStream = new DepthStreamRenderer(this);

                    if (null ==this.skeletonStream)
                        this.skeletonStream = new SkeletonStreamRenderer(this, this.SkeletonToDepthMap);
            
                    // Update Depth and Skeleton Stream size and location based on the back-buffer
                    this.UpdateStreamSizeAndLocation();
                }

                // Update the avatar renderer
                if (null != this.animator)
                {
                    this.animator.SkeletonDrawn = false;
                }

            this.HandleInput();
            this.UpdateCamera(gameTime);

            base.Update(gameTime);
        }

        /// <summary>
        /// Create the viewing camera.
        /// </summary>
        protected void UpdateViewingCamera()
        {
            GraphicsDevice device = this.graphics.GraphicsDevice;

            // Compute camera matrices.
            this.view = Matrix.CreateTranslation(0, -CameraHeight, 0) *
                          Matrix.CreateRotationY(MathHelper.ToRadians(this.cameraRotation)) *
                          Matrix.CreateRotationX(MathHelper.ToRadians(this.cameraArc)) *
                          Matrix.CreateLookAt(
                                                new Vector3(0, 0, -this.cameraDistance),
                                                new Vector3(0, 0, 0), 
                                                Vector3.Up);

            // Kinect vertical FOV in degrees
            float nominalVerticalFieldOfView = 45.6f;

            if (null != this.chooser && null != this.Chooser.Sensor && this.Chooser.Sensor.IsOpen &&  this.Chooser.Sensor.IsOpen)
            {
                PointF[] depthVisualization = this.Chooser.Sensor.CoordinateMapper.GetDepthFrameToCameraSpaceTable();
            }

            this.projection = Matrix.CreatePerspectiveFieldOfView(
                                                                nominalVerticalFieldOfView * (float)Math.PI / 180.0f,
                                                                device.Viewport.AspectRatio,
                                                                1,
                                                                10000);
        }

        #endregion

        #region Draw
        
        /// <summary>
        /// This method renders the current state.
        /// </summary>
        /// <param name="gameTime">The elapsed game time.</param>
        protected override void Draw(GameTime gameTime)
        {
            // Clear the screen
            GraphicsDevice.Clear(Color.White);

            this.UpdateViewingCamera();

            // Render the depth and skeleton stream
            if (null != this.depthStream && null != this.skeletonStream)
            {
                this.depthStream.Draw(gameTime);
                this.skeletonStream.Draw(gameTime);
            }

            // Optionally draw a ground plane grid and world axes that the avatar stands on.
            // For our axes, red is +X, green is +Y, blue is +Z
            if (this.drawGrid && null != this.planarXzGrid && null != this.worldAxes)
            {
                this.planarXzGrid.Draw(gameTime, Matrix.Identity, this.view, this.projection);
                this.worldAxes.Draw(gameTime, Matrix.Identity, this.view, this.projection);
            }

            // Draw the actual avatar
            if (null != this.animator && (!this.drawAvatarOnlyWhenPlayerDetected || (this.drawAvatarOnlyWhenPlayerDetected && this.skeletonDetected)))
            {
                //Debug.WriteIf(this.gameLoopCounter % 99 == 0, "...animator READY!!" + gameTime);
                this.animator.Draw(gameTime, Matrix.Identity, this.view, this.projection);
            }
            else
            {
                Debug.WriteIf(this.gameLoopCounter%9==0,"animator avatar NOT ready Draw method not working");
            }

            // Render header/footer image
            this.SharedSpriteBatch.Begin();
            this.SharedSpriteBatch.Draw(this.header, Vector2.Zero, null, Color.White);
            this.SharedSpriteBatch.End();

            base.Draw(gameTime);
        }
        
        #endregion

        #region Handle Input

        /// <summary>
        /// Handles input for quitting the game.
        /// </summary>
        private void HandleInput()
        {
            this.currentKeyboard = Keyboard.GetState();

            // Check for exit.
            if (this.currentKeyboard.IsKeyDown(Keys.Escape))
            {
                this.Exit();
            }

            // Fullscreen on/off toggle
            if (this.currentKeyboard.IsKeyDown(Keys.F11))
            {
                // If not down last update, key has just been pressed.
                if (!this.previousKeyboard.IsKeyDown(Keys.F11))
                {
                    this.fullscreenMode = !this.fullscreenMode;
                    this.SetScreenMode();
                }
            }

            // Draw avatar when not detected on/off toggle
            if (this.currentKeyboard.IsKeyDown(Keys.V))
            {
                if (!this.previousKeyboard.IsKeyDown(Keys.V))
                {
                    this.drawAvatarOnlyWhenPlayerDetected = !this.drawAvatarOnlyWhenPlayerDetected;
                }
            }

            // Seated and near mode on/off toggle
            if (this.currentKeyboard.IsKeyDown(Keys.N))
            {
                if (!this.previousKeyboard.IsKeyDown(Keys.N))
                {
                    this.chooser.SeatedMode = !this.chooser.SeatedMode;
                    this.skeletonDetected = false;

                    // Set near mode to accompany seated mode
                    this.chooser.NearMode = this.chooser.SeatedMode;
                }
            }

            // Fix the avatar hip center draw height on/off toggle
            if (this.currentKeyboard.IsKeyDown(Keys.H))
            {
                if (!this.previousKeyboard.IsKeyDown(Keys.H))
                {
                    this.fixAvatarSpineBaseDrawHeight = !this.fixAvatarSpineBaseDrawHeight;
                }
            }

            // Fix the avatar leaning back too much on/off toggle
            if (this.currentKeyboard.IsKeyDown(Keys.L))
            {
                if (!this.previousKeyboard.IsKeyDown(Keys.L))
                {
                    this.leanAdjust = !this.leanAdjust;
                }
            }

            // Reset the avatar filters (also resets camera)
            if (this.currentKeyboard.IsKeyDown(Keys.R))
            {
                if (!this.previousKeyboard.IsKeyDown(Keys.R))
                {
                    if (null != this.animator)
                    {
                        this.animator.Reset();
                    }
                }
            }
        }

        /// <summary>
        /// Toggle between fullscreen and windowed mode
        /// </summary>
        private void SetScreenMode()
        {
            // This sets the display resolution or window size to the desired size
            // If windowed, it also forces a 4:3 ratio for height and adds 110 for header/footer
            if (this.fullscreenMode)
            {
                foreach (DisplayMode mode in GraphicsAdapter.DefaultAdapter.SupportedDisplayModes)
                {
                    // Check our requested FullScreenWidth and Height against each supported display mode and set if valid
                    if ((mode.Width == FullScreenWidth) && (mode.Height == FullScreenHeight))
                    {
                        this.graphics.PreferredBackBufferWidth = FullScreenWidth;
                        this.graphics.PreferredBackBufferHeight = FullScreenHeight;
                        this.graphics.IsFullScreen = true;
                        this.graphics.ApplyChanges();
                    }
                }
            }
            else
            {
                if (WindowedWidth <= GraphicsAdapter.DefaultAdapter.CurrentDisplayMode.Width)
                {
                    this.graphics.PreferredBackBufferWidth = WindowedWidth;
                    this.graphics.PreferredBackBufferHeight = ((WindowedWidth / 4) * 3) + 110;
                    this.graphics.IsFullScreen = false;
                    this.graphics.ApplyChanges();
                }
            }

            this.UpdateStreamSizeAndLocation();
        }

        /// <summary>
        /// Update the depth and skeleton stream rendering position and size based on the backbuffer resolution.
        /// </summary>
        private void UpdateStreamSizeAndLocation()
        {
            Debug.WriteLine("UpdateStreamSizeAndLocation" + depthStream + skeletonStream);
            int depthStreamWidth = this.graphics.PreferredBackBufferWidth / 4;
            Vector2 size = new Vector2(depthStreamWidth, (depthStreamWidth / 4) * 3);
            //Vector2 size = new Vector2(depthStream.depthWidth, depthStream.depthHeight);
            Vector2 pos = new Vector2(this.graphics.PreferredBackBufferWidth - depthStreamWidth - 10, 85);

            if (null != this.depthStream)
            {
                this.depthStream.Size = size;
                this.depthStream.Position = pos;
            }

            if (null != this.skeletonStream)
            {
                this.skeletonStream.Size = size;
                this.skeletonStream.Position = pos;
            }
        }

        /// <summary>
        /// Handles camera input.
        /// </summary>
        /// <param name="gameTime">The gametime.</param>
        private void UpdateCamera(GameTime gameTime)
        {
            float time = (float)gameTime.ElapsedGameTime.TotalMilliseconds;

            // Check for input to rotate the camera up and down around the model.
            if (this.currentKeyboard.IsKeyDown(Keys.Up) ||
                this.currentKeyboard.IsKeyDown(Keys.W))
            {
                this.cameraArc += time * CameraArcIncrement;
            }
            
            if (this.currentKeyboard.IsKeyDown(Keys.Down) ||
                this.currentKeyboard.IsKeyDown(Keys.S))
            {
                this.cameraArc -= time * CameraArcIncrement;
            }

            // Limit the arc movement.
            if (this.cameraArc > CameraArcAngleLimit)
            {
                this.cameraArc = CameraArcAngleLimit;
            }
            else if (this.cameraArc < -CameraArcAngleLimit)
            {
                this.cameraArc = -CameraArcAngleLimit;
            }

            // Check for input to rotate the camera around the model.
            if (this.currentKeyboard.IsKeyDown(Keys.Right) ||
                this.currentKeyboard.IsKeyDown(Keys.D))
            {
                this.cameraRotation += time * CameraArcIncrement;
            }

            if (this.currentKeyboard.IsKeyDown(Keys.Left) ||
                this.currentKeyboard.IsKeyDown(Keys.A))
            {
                this.cameraRotation -= time * CameraArcIncrement;
            }

            // Check for input to zoom camera in and out.
            if (this.currentKeyboard.IsKeyDown(Keys.Z))
            {
                this.cameraDistance += time * CameraZoomIncrement;
            }

            if (this.currentKeyboard.IsKeyDown(Keys.X))
            {
                this.cameraDistance -= time * CameraZoomIncrement;
            }

            // Limit the camera distance from the origin.
            if (this.cameraDistance > CameraMaxDistance)
            {
                this.cameraDistance = CameraMaxDistance;
            }
            else if (this.cameraDistance < CameraMinDistance)
            {
                this.cameraDistance = CameraMinDistance;
            }

            if (this.currentKeyboard.IsKeyDown(Keys.R))
            {
                this.cameraArc = 0;
                this.cameraRotation = 0;
                this.cameraDistance = CameraStartingTranslation;
            }
        }

        #endregion

        #region AvatarRetargeting

        /// <summary>
        /// 3D avatar models typically have varying bone structures and joint orientations, depending on how they are built.
        /// Here we adapt the calculated hierarchical relative rotation matrices to work with our avatar and set these into the 
        /// boneTransforms array. This array is then later converted to world transforms and then skinning transforms for the
        /// XNA skinning processor to draw the mesh.
        /// The "Dude.fbx" model defines more bones/joints (57 in total) and in different locations and orientations to the 
        /// Nui Skeleton. Many of the bones/joints have no direct equivalent - e.g. with Kinect we cannot currently recover 
        /// the fingers pose. Bones are defined relative to each other, hence unknown bones will be left as identity relative
        /// transformation in the boneTransforms array, causing them to take their parent's orientation in the world coordinate system.
        /// </summary>
        /// <param name="skeleton">The Kinect skeleton.</param>
        /// <param name="bindRoot">The bind root matrix of the avatar mesh.</param>
        /// <param name="boneTransforms">The avatar mesh rotation matrices.</param>
        private void RetargetMatrixHierarchyToAvatarMesh(Body skeleton, Matrix bindRoot, Matrix[] boneTransforms)
        {
            if (null == skeleton)
            {
                Debug.WriteLine("skeleton is dead in the RetargetMatrixHierarchyToAvatarMesh");
                return;
            }

            // Set the bone orientation data in the avatar mesh
            foreach (JointType jointType in skeleton.JointOrientations.Keys)
            {
                // If any of the joints/bones are not tracked, skip them
                // Note that if we run filters on the raw skeleton data, which fix tracking problems,
                // We should set the tracking state from NotTracked to Inferred.
                if (skeleton.Joints[jointType].TrackingState == TrackingState.NotTracked)
                {
                    continue;
                }
                // The avatar mesh rotation matrices.
                this.SetJointTransformation(skeleton.JointOrientations[jointType], skeleton, bindRoot, ref boneTransforms);
            }

            // If seated mode is on, sit the avatar down
            if (this.Chooser.SeatedMode && this.setSeatedPostureInSeatedMode)
            {
                Debug.WriteLine("RetargetMatrixHierarchyToAvatarMesh + setSeatedPostureInSeatedMode=" + boneTransforms);
                this.SetSeatedPosture(ref boneTransforms);
            }

            // Set the world position of the avatar
            this.SetAvatarRootWorldPosition(skeleton, ref boneTransforms);
        }
        
        /// <summary>
        /// Set the bone transform in the avatar mesh.
        /// </summary>
        /// <param name="bone">Nui Joint/bone orientation</param>
        /// <param name="skeleton">The Kinect skeleton.</param>
        /// <param name="bindRoot">The bind root matrix of the avatar mesh.</param>
        /// <param name="boneTransforms">The avatar mesh rotation matrices.</param>
        private Matrix processBone(JointOrientation bone, Body skeleton, Matrix bindRoot, ref Matrix[] boneTransforms)
        {
            Matrix tempMat = KinectHelper.JointOrientationToXNAMatrix(bone.Orientation);
            // If the joint has a parent, constrain the bone direction to be within the constraint cone
            JointType parentIdx = KinectHelper.ParentBoneJoint(bone.JointType);
            Joint parentJoint = new Joint();
            skeleton.Joints.TryGetValue(parentIdx, out parentJoint);
            int meshJointId = 0;
            bool success = this.nuiJointToAvatarBoneIndex.TryGetValue(bone.JointType, out meshJointId);
            Matrix avatarBoneMatrix = boneTransforms[meshJointId];

            // If the joint has a parent, constrain the bone direction to be within the constraint cone
            //JointType parentIdx = skeleton.BoneOrientations[jc.Joint].StartJoint;
            //skeleton.JointOrientations.Values;
            // Local bone orientation relative to parent
            // borrowed from drawing the skeleton in BoneOrientation 
            Matrix boneOrientationRelativeToParent = KinectHelper.JointOrientationToXNAMatrix(skeleton.JointOrientations[parentJoint.JointType].Orientation);
            Quaternion childQ = QuaternionFromVector4(bone.Orientation);

            // Kinect = +X left, +Y up, +Z forward in body coordinate system
            // Avatar = +Z left, +X up, +Y forward
            Vector3 boneRelDirVecLs = new Vector3(boneOrientationRelativeToParent.M21, boneOrientationRelativeToParent.M22, boneOrientationRelativeToParent.M23);
            boneRelDirVecLs.Normalize();

            Quaternion parentQ = QuaternionFromVector4(skeleton.JointOrientations[parentJoint.JointType].Orientation);
            Quaternion parentQInverse = new Quaternion();
            Quaternion.Inverse(ref parentQ, out parentQInverse);

            Vector3 boneDirWs = KinectHelper.JointOrientationToVector3(bone);
            boneDirWs.Normalize();

            Vector3 bindRootscale = new Vector3();
            Vector3 bindRoottranslation = new Vector3();
            Quaternion bindRootQ = new Quaternion();
            //Quaternion.CreateFromRotationMatrix(ref bindRoot, out bindRootQ);
            bool worked = bindRoot.Decompose(out bindRootscale, out bindRootQ, out bindRoottranslation);

            //Do remember to always multiply the matrix in Scale, Rotation, Translation (SRT) order!
            Quaternion newRotation = bindRootQ * parentQInverse * childQ;

            Matrix.Transform(avatarBoneMatrix, childQ);





            Quaternion kinectRotation = KinectHelper.DecomposeMatRot(tempMat);    // XYZ
            Quaternion avatarRotation = new Quaternion(kinectRotation.Y, -kinectRotation.Z, -kinectRotation.X, kinectRotation.W); // transform from Kinect to avatar coordinate system
            tempMat = Matrix.CreateFromQuaternion(avatarRotation);

            Matrix hipOrientation = KinectHelper.JointOrientationToXNAMatrix(bone.Orientation);
            hipOrientation.Translation = avatarBoneMatrix.Translation;
            Quaternion jointOrientation = QuaternionFromVector4(bone.Orientation);

            // print local rotations using inverse parent * child
            //Quaternion spineMidLocal = Quaternion.Inverse(spineBaseOrientation) * spineMidOrientation;

            // Kinect = +X left, +Y up, +Z forward in body coordinate system
            // Avatar = +Z left, +X up, +Y forward
            kinectRotation = KinectHelper.DecomposeMatRot(hipOrientation);    // XYZ
            avatarRotation = new Quaternion(kinectRotation.Y, kinectRotation.Z, kinectRotation.X, kinectRotation.W); // transform from Kinect to avatar coordinate system
            Matrix combined = Matrix.CreateFromQuaternion(avatarRotation);
            Matrix invBindRoot = bindRoot;
            invBindRoot.Translation = Vector3.Zero;
            invBindRoot = Matrix.Invert(invBindRoot); 

            // Here we scale the translation, as the "Dude" avatar mesh is defined in centimeters, and the Kinect skeleton joint positions in meters.
            Vector3 transVec = Vector3.Zero;
            Vector3 bindRootBTranslation = bindRoot.Translation;
            Vector3 scaleSkeletonToAvatar = new Vector3(-4f, 4f, 4f);
            // Get the constraint direction in world space from the parent orientation
            Matrix matConstraintLocalToWorld = KinectHelper.JointOrientationToXNAMatrix(bone.Orientation);
            transVec = KinectHelper.SkeletonPointToVector3(skeleton.Joints[bone.JointType].Position);
            transVec.Normalize();

            Matrix pelvis = boneTransforms[meshJointId];
           
            Matrix invPelvis = Matrix.Invert(pelvis);

            Matrix localWorld = KinectHelper.JointOrientationToXNAMatrix(bone.Orientation);
            Vector3 jointPosVec = KinectHelper.Position(skeleton, bone.JointType);
            
            jointPosVec *= scaleSkeletonToAvatar; // This will scale and optionally mirror the skeleton
            localWorld.Translation = jointPosVec; // set the translation into the rotation matrix

           // this.localJointCoordinateSystemCrosses.Draw(gameTime, localWorld * world, view, projection);

            //Matrix tempMat = Matrix.CreateTranslation(transVec * bindRootBTranslation);
            //Matrix tempMat = Matrix.CreateTranslation(transVec * SkeletonTranslationScaleFactor);
            //Matrix tempMat = KinectHelper.JointOrientationToXNAMatrix(bone.Orientation);
            //Quaternion quaternionBone2 = KinectHelper.QuaternionFromVector4(bone.Orientation);
            Quaternion quaternionBone = KinectHelper.Vector4ToXNAQuaternion(bone.Orientation);
            Quaternion quaternionBoneInv = Quaternion.Inverse(quaternionBone);
            
            Matrix thisMatrixFromQB = Matrix.CreateFromQuaternion(quaternionBone);
            return combined;
        }


                /// <summary>
        /// get the Matrix rotation/translation for the current (Source)Kinect Skeleton to (Target)XNA Avatar.
        /// Quaternion newRotation = bindRotation * parentQ.Inverse() * childQ;
        /// </summary>
        /// <param name="bone">Nui Joint/bone orientation</param>
        /// <param name="skeleton">The Kinect skeleton.</param>
        /// <param name="bindRoot">The bind root matrix of the avatar mesh.</param>
        /// <param name="boneTransforms">The avatar mesh rotation matrices.</param>
        private Matrix processReplaceBoneMatrixforAvatarRotation(JointOrientation bone, Body skeleton, Matrix bindRoot, ref Matrix[] boneTransforms)
        {
            // If the joint has a parent, constrain the bone direction to be within the constraint cone
            JointType parentIdx = KinectHelper.ParentBoneJoint(bone.JointType);
            Joint parentJoint =  new Joint();
            skeleton.Joints.TryGetValue(parentIdx,out parentJoint);
            int meshJointId = 0;
            bool success = this.nuiJointToAvatarBoneIndex.TryGetValue(bone.JointType, out meshJointId);
            Matrix avatarBoneMatrix = boneTransforms[meshJointId];

            Matrix invBindRoot = bindRoot;
            //invBindRoot.Translation = Vector3.Zero;
            invBindRoot = Matrix.Invert(invBindRoot); 

            // If the joint has a parent, constrain the bone direction to be within the constraint cone
            //JointType parentIdx = skeleton.BoneOrientations[jc.Joint].StartJoint;
            //skeleton.JointOrientations.Values;
            // Local bone orientation relative to parent
            // borrowed from drawing the skeleton in BoneOrientation 
            Matrix boneOrientationRelativeToParent = KinectHelper.JointOrientationToXNAMatrix(skeleton.JointOrientations[parentJoint.JointType].Orientation);
            Quaternion childQ = QuaternionFromVector4(bone.Orientation);

            Vector3 boneRelDirVecLs = new Vector3(boneOrientationRelativeToParent.M21, boneOrientationRelativeToParent.M22, boneOrientationRelativeToParent.M23);
            boneRelDirVecLs.Normalize();

            Quaternion parentQ = QuaternionFromVector4(skeleton.JointOrientations[parentJoint.JointType].Orientation);
            Quaternion parentQInverse = new Quaternion();
            Quaternion.Inverse(ref parentQ, out parentQInverse);

            Vector3 boneDirWs = KinectHelper.JointOrientationToVector3(bone);
            boneDirWs.Normalize();

            Vector3 bindRootscale = new Vector3();
            Vector3 bindRoottranslation = new Vector3();
            Quaternion bindRootQ = new Quaternion();
            //Quaternion.CreateFromRotationMatrix(ref bindRoot, out bindRootQ);
            bool worked = bindRoot.Decompose(out bindRootscale, out bindRootQ, out bindRoottranslation);

            //Do remember to always multiply the matrix in Scale, Rotation, Translation (SRT) order!
            Quaternion newRotation = bindRootQ * parentQInverse * childQ;

            Matrix.Transform(avatarBoneMatrix, childQ);

            Matrix tempMat = KinectHelper.JointOrientationToXNAMatrix(bone.Orientation);
            // Kinect = +X left, +Y up, +Z forward in body coordinate system
            // Avatar = +Z left, +X up, +Y forward
            Quaternion kinectRotation = KinectHelper.DecomposeMatRot(tempMat);    // XYZ
            Quaternion avatarRotation = new Quaternion(kinectRotation.Y, kinectRotation.Z, kinectRotation.X, kinectRotation.W); // transform from Kinect to avatar coordinate system
            //tempMat = Matrix.CreateFromQuaternion(avatarRotation);

            Matrix hipOrientation = KinectHelper.JointOrientationToXNAMatrix(bone.Orientation);
            hipOrientation.Translation = avatarBoneMatrix.Translation;
            // We can use the same method as in HipCenter above to invert the root and pelvis bind pose,
            // however, alternately we can also explicitly swap axes and adjust the rotations to get from
            // the Kinect rotation to the model hip orientation, similar to what we do for the following joints/bones.
            Quaternion jointOrientation = QuaternionFromVector4(bone.Orientation);

            // print local rotations using inverse parent * child
            //Quaternion spineMidLocal = Quaternion.Inverse(spineBaseOrientation) * spineMidOrientation;

            // Kinect = +X left, +Y up, +Z forward in body coordinate system
            // Avatar = +Z left, +X up, +Y forward
            kinectRotation = KinectHelper.DecomposeMatRot(hipOrientation);    // XYZ
            avatarRotation = new Quaternion(kinectRotation.Y, kinectRotation.Z, kinectRotation.X, kinectRotation.W); // transform from Kinect to avatar coordinate system
            Matrix combined = Matrix.CreateFromQuaternion(avatarRotation);
            
            //combined.Translation = avatarBoneMatrix.Translation;
            //// Add a small adjustment rotation to manually correct for the rotation in the parent bind
            //// pose node in the model mesh - this can be found by looking in the FBX or in 3DSMax/Maya.
            //Matrix adjustment = Matrix.CreateRotationY(MathHelper.ToRadians(-90));
            //combined *= adjustment;
            //Matrix adjustment2 = Matrix.CreateRotationX(MathHelper.ToRadians(-90));
            //combined *= adjustment2;
            //Matrix adjustment = Matrix.Identity;
            //if (bone.JointType == JointType.HandLeft){
            //    adjustment = Matrix.CreateRotationY(MathHelper.ToRadians(-90));  // -90 degree rotation around the local Kinect y axis for the wrist-hand bone.
            //    combined = Matrix.Transform(boneTransforms[meshJointId] * adjustment, parentQInverse * childQ);
            //}
            //else
                combined = Matrix.Transform(boneTransforms[meshJointId], parentQInverse * childQ); ; 
            //if (bone.JointType == JointType.HandLeft)
            //    combined *= adjustment;

                combined = (invBindRoot * boneOrientationRelativeToParent) * Matrix.Invert(tempMat);

            Debug.WriteLineIf(bone.JointType.ToString().Contains("Left"), "bone left "+bone.JointType.ToString()+" = " + combined.Translation + " compare to avatarBoneMatrix" + avatarBoneMatrix.Translation);


            return combined;
        }

        private Vector3 GetVector3FromCameraSpacePoint(CameraSpacePoint point)
        {
            return KinectHelper.SkeletonPointToVector3(point);
        }
        private Quaternion QuaternionFromVector4(Microsoft.Kinect.Vector4 boneOrientation)
        {
            return KinectHelper.Vector4ToXNAQuaternion(boneOrientation);
        }
        private Matrix processWholeBody(JointOrientation bone, Body body, Matrix bindRoot, ref Matrix[] boneTransforms)
        {
            // get positions from kinect
            Vector3 neckPosition = GetVector3FromCameraSpacePoint(body.Joints[JointType.Neck].Position);
            Vector3 headPosition = GetVector3FromCameraSpacePoint(body.Joints[JointType.Head].Position);
            Vector3 leftHipPosition = GetVector3FromCameraSpacePoint(body.Joints[JointType.HipLeft].Position);
            Vector3 spineBasePosition = GetVector3FromCameraSpacePoint(body.Joints[JointType.SpineBase].Position);
            Vector3 spineMidPosition = GetVector3FromCameraSpacePoint(body.Joints[JointType.SpineMid].Position);
            Vector3 spineShoulderPosition = GetVector3FromCameraSpacePoint(body.Joints[JointType.SpineShoulder].Position);
            Vector3 leftShoulderPosition = GetVector3FromCameraSpacePoint(body.Joints[JointType.ShoulderLeft].Position);
            Vector3 leftElbowPosition = GetVector3FromCameraSpacePoint(body.Joints[JointType.ElbowLeft].Position);
            Vector3 leftHandPosition = GetVector3FromCameraSpacePoint(body.Joints[JointType.HandLeft].Position);
            Vector3 leftWristPosition = GetVector3FromCameraSpacePoint(body.Joints[JointType.WristLeft].Position);
            Vector3 leftHandTipPosition = GetVector3FromCameraSpacePoint(body.Joints[JointType.HandTipLeft].Position);
            Vector3 leftThumbPosition = GetVector3FromCameraSpacePoint(body.Joints[JointType.ThumbLeft].Position);
            Vector3 rightShoulderPosition = GetVector3FromCameraSpacePoint(body.Joints[JointType.ShoulderRight].Position);
            Vector3 rightElbowPosition = GetVector3FromCameraSpacePoint(body.Joints[JointType.ElbowRight].Position);

            int meshJointId = 0;
            bool success = this.nuiJointToAvatarBoneIndex.TryGetValue(bone.JointType, out meshJointId);
            //            this.nuiJointToAvatarBoneIndex.Add(JointType.SpineShoulder, 6);
            //this.nuiJointToAvatarBoneIndex.Add(JointType.Head, 7);
            //this.nuiJointToAvatarBoneIndex.Add(JointType.ShoulderLeft, 12);
            //this.nuiJointToAvatarBoneIndex.Add(JointType.ElbowLeft, 13);
            //this.nuiJointToAvatarBoneIndex.Add(JointType.WristLeft, 14);
            //this.nuiJointToAvatarBoneIndex.Add(JointType.HandLeft, 15);
            //52 = Left Ankle

            //Vector3 scale = new Vector3(-25f, -4f, .9f);
            Vector3 scale = boneTransforms[50].Translation / leftHipPosition;
            Debug.WriteLine(" scale = " + scale.X + " Y=" + scale.Y + " Z=" + scale.Z);
            // change or delta ∆ always is calculated as final minus initial value
            Debug.WriteLine("skel SpineShoulder x=" + spineShoulderPosition.X + " .Y=" + spineShoulderPosition.Y + " Z=" + spineShoulderPosition.Z );
            Debug.WriteLine("avat SpineShoulder x=" + boneTransforms[6].Translation.X + " .Y=" + boneTransforms[6].Translation.Y + " Z=" + boneTransforms[6].Translation.Z);
            // left shoulder to spine just for fun... maybe used later. fixed body style like Transformer not like humaniod that can bend shoulder in or maybe... 
            Debug.WriteLine("skel leftShold x=" + leftShoulderPosition.X + " Y=" + leftShoulderPosition.Y + " Z=" + leftShoulderPosition.Z);
            Debug.WriteLine("avat leftShold x=" + boneTransforms[12].Translation.X + " .Y=" + boneTransforms[12].Translation.Y + " Z=" + boneTransforms[12].Translation.Z);
            Vector3 leftSholderDir = new Vector3(leftShoulderPosition.X - spineShoulderPosition.X, leftShoulderPosition.Y - spineShoulderPosition.Y, leftShoulderPosition.Z - spineShoulderPosition.Z);
            Debug.WriteLine("spinetoleftsholdr DIR=" + leftSholderDir.Length() +" subtract two=" + (leftShoulderPosition-spineShoulderPosition).Length());
            // upper left arm wrist - elbow
            Debug.WriteLine("skel ElbowLeft x=" + leftElbowPosition.X + " .Y=" + leftElbowPosition.Y + " z=" + leftElbowPosition.Z );
            Debug.WriteLine("avat ElbowLeft x=" + boneTransforms[13].Translation.X + " .Y=" + boneTransforms[13].Translation.Y + " Z=" + boneTransforms[13].Translation.Z);
            Vector3 leftUpArmDir = new Vector3(leftElbowPosition.X - leftShoulderPosition.X, leftElbowPosition.Y - leftShoulderPosition.Y, leftElbowPosition.Z - leftShoulderPosition.Z) *scale;
            Debug.WriteLine("leftUpArmDir DIR=" + leftUpArmDir.Length());
            //leftUpArmDir.Normalize();
            // ElbowLeft = boneTransforms[13]
            boneTransforms[13].Translation = KinectHelper.moveAvatarWorldPosition(boneTransforms[13], leftUpArmDir);
            Debug.WriteLine("AFTER avat ElbowLeft x=" + boneTransforms[13].Translation.X + " .Y=" + boneTransforms[13].Translation.Y + " Z=" + boneTransforms[13].Translation.Z);
            // lower left arm wrist = boneTransforms[14] - elbow = boneTransforms[13]
            Debug.WriteLine("skel WristLeft x=" + leftWristPosition.X + " .Y=" + leftWristPosition.Y + " Z=" + leftWristPosition.Z);
            Debug.WriteLine("avat WristLeft x=" + boneTransforms[14].Translation.X + " .Y=" + boneTransforms[14].Translation.Y + " Z=" + boneTransforms[14].Translation.Z);
            Vector3 leftLowArmDir = new Vector3(leftWristPosition.X - leftElbowPosition.X, leftWristPosition.Y - leftElbowPosition.Y, leftWristPosition.Z - leftElbowPosition.Z) * scale;
            Debug.WriteLine("leftUpArmDir DIR=" + leftUpArmDir.Length());
            //leftLowArmDir.Normalize();
            //move the bone position of the wrist
            //Vector3 newWristPos =
            //boneTransforms[14].Translation = KinectHelper.moveAvatarWorldPosition(boneTransforms[14], leftLowArmDir);
            //new Vector3(boneTransforms[13].Translation.X + leftLowArmDir.X, boneTransforms[13].Translation.Y + leftLowArmDir.Y, boneTransforms[14].Translation.Z);
            Debug.WriteLine("AFTER avat WristLeft x=" + boneTransforms[14].Translation.X + " .Y=" + boneTransforms[14].Translation.Y + " Z=" + boneTransforms[14].Translation.Z);


            Debug.WriteLine("skel HandLeft x=" + leftHandPosition.X + " .Y=" + leftHandPosition.Y + " Z=" + leftHandPosition.Z);
            Debug.WriteLine("avat HandLeft x=" + boneTransforms[15].Translation.X + " .Y=" + boneTransforms[15].Translation.Y + " Z=" + boneTransforms[15].Translation.Z);
            // reposition hand 
            Vector3 newHandDir = new Vector3(leftHandPosition.X - leftWristPosition.X, leftHandPosition.Y - leftWristPosition.Y, leftHandPosition.Z - leftWristPosition.Z) * scale;
            // joint position of the hand (15) with the wrist(14)
           //boneTransforms[15].Translation = KinectHelper.moveAvatarWorldPosition(boneTransforms[15], newHandDir);
            Debug.WriteLine("AFTER avat HandLeft x=" + boneTransforms[15].Translation.X + " .Y=" + boneTransforms[15].Translation.Y + " Z=" + boneTransforms[15].Translation.Z);
            // get rotations from kinect
            Quaternion neckOrientation = QuaternionFromVector4(body.JointOrientations[JointType.Neck].Orientation);
            Quaternion headOrientation = QuaternionFromVector4(body.JointOrientations[JointType.Head].Orientation);
            Quaternion leftHipOrientation = QuaternionFromVector4(body.JointOrientations[JointType.HipLeft].Orientation);
            Quaternion spineBaseOrientation = QuaternionFromVector4(body.JointOrientations[JointType.SpineBase].Orientation);
            Quaternion spineMidOrientation = QuaternionFromVector4(body.JointOrientations[JointType.SpineMid].Orientation);
            Quaternion spineShoulderOrientation = QuaternionFromVector4(body.JointOrientations[JointType.SpineShoulder].Orientation);
            Quaternion leftShoulderOrientation = QuaternionFromVector4(body.JointOrientations[JointType.ShoulderLeft].Orientation);
            Quaternion leftElbowOrientation = QuaternionFromVector4(body.JointOrientations[JointType.ElbowLeft].Orientation);
            Quaternion leftHandOrientation = QuaternionFromVector4(body.JointOrientations[JointType.HandLeft].Orientation);
            Quaternion leftWristOrientation = QuaternionFromVector4(body.JointOrientations[JointType.WristLeft].Orientation);
            Quaternion leftHandTipOrientation = QuaternionFromVector4(body.JointOrientations[JointType.HandTipLeft].Orientation);
            Quaternion leftThumbOrientation = QuaternionFromVector4(body.JointOrientations[JointType.ThumbLeft].Orientation);
            Quaternion rightShoulderOrientation = QuaternionFromVector4(body.JointOrientations[JointType.ShoulderRight].Orientation);
            Quaternion rightElbowOrientation = QuaternionFromVector4(body.JointOrientations[JointType.ElbowRight].Orientation);
            Quaternion rightHandOrientation = QuaternionFromVector4(body.JointOrientations[JointType.HandRight].Orientation);
            Quaternion rightWristOrientation = QuaternionFromVector4(body.JointOrientations[JointType.WristRight].Orientation);
            Quaternion rightHandTipOrientation = QuaternionFromVector4(body.JointOrientations[JointType.HandTipRight].Orientation);
            Quaternion rightThumbOrientation = QuaternionFromVector4(body.JointOrientations[JointType.ThumbRight].Orientation);

            // calculate bones from positions
            Vector3 spineBone = spineMidPosition - spineBasePosition;
            Vector3 neckBone = neckPosition - spineShoulderPosition;
            Vector3 headBone = headPosition - neckPosition;
            Vector3 chestBone = spineShoulderPosition - spineMidPosition;
            Vector3 leftShoulderBone = leftShoulderPosition - spineShoulderPosition;
            Vector3 leftArmBone = leftElbowPosition - leftShoulderPosition;
            Vector3 rightShoulderBone = rightShoulderPosition - spineShoulderPosition;
            Vector3 rightArmBone = rightElbowPosition - rightShoulderPosition;

            // print local rotations using inverse parent * child
            Quaternion spineMidLocal = Quaternion.Inverse(spineBaseOrientation) * spineMidOrientation;
            Quaternion spineShoulderLocal = Quaternion.Inverse(spineMidOrientation) * spineShoulderOrientation;
            Quaternion leftShoulderLocal = Quaternion.Inverse(spineShoulderOrientation) * leftShoulderOrientation;
            Quaternion leftElbowLocal = Quaternion.Inverse(leftShoulderOrientation) * leftElbowOrientation;
            Quaternion neckLocal = Quaternion.Inverse(spineShoulderOrientation) * neckOrientation;
            Quaternion headLocal = Quaternion.Inverse(neckOrientation) * headOrientation;
           // Debug.WriteLine("kinect local rotation spine mid = " + KinectHelper.QuaternionTo ean(spineMidLocal).ToString());
          //  Debug.WriteLine("kinect local rotation spine shoulder = " + KinectHelper.QuaternionToEuclidean(spineShoulderLocal).ToString());
          //  Debug.WriteLine("kinect local rotation left shoulder = " + KinectHelper.QuaternionToEuclidean(leftShoulderLocal).ToString());
         //   Debug.WriteLine("kinect local rotation left arm = " + KinectHelper.QuaternionToEuclidean(leftElbowLocal).ToString());
          //  Debug.WriteLine("kinect local rotation neck = " + KinectHelper.QuaternionToEuclidean(neckLocal).ToString());
           // Debug.WriteLine("kinect local rotation head = " + KinectHelper.QuaternionToEuclidean(headLocal).ToString());

            // get kinect vector4. 
            Microsoft.Kinect.Vector4 neckbone = body.JointOrientations[JointType.Neck].Orientation;
            Microsoft.Kinect.Vector4 SpineShoulder = body.JointOrientations[JointType.SpineShoulder].Orientation;
            Microsoft.Kinect.Vector4 SpineMid = body.JointOrientations[JointType.SpineMid].Orientation;
            Microsoft.Kinect.Vector4 ShoulderLeft = body.JointOrientations[JointType.ShoulderLeft].Orientation;
            Microsoft.Kinect.Vector4 ElbowLeft = body.JointOrientations[JointType.ElbowLeft].Orientation;
            Microsoft.Kinect.Vector4 WristLeft = body.JointOrientations[JointType.WristLeft].Orientation;
            Microsoft.Kinect.Vector4 HandLeft = body.JointOrientations[JointType.HandLeft].Orientation;

            //Debug.WriteLine("neckboneY=" + neckbone.Y + " Z=" + neckbone.Z + " X=" + neckbone.X + " W="+ neckbone.W);
            //Debug.WriteLine("SpineShoulder Y=" + SpineShoulder.Y + " Z=" + SpineShoulder.Z + " X=" + SpineShoulder.X + " W=" + SpineShoulder.W);
            //Debug.WriteLine("SpineMid Y=" + SpineMid.Y + " Z=" + SpineMid.Z + " X=" + SpineMid.X + " W=" + SpineMid.W);
            //Debug.WriteLine("ShoulderLeft Y=" + ShoulderLeft.Y + " Z=" + ShoulderLeft.Z + " X=" + ShoulderLeft.X + " W=" + ShoulderLeft.W);
            //Debug.WriteLine("ElbowLeft Y=" + ElbowLeft.Y + " Z=" + ElbowLeft.Z + " X=" + ElbowLeft.X + " W=" + ElbowLeft.W);
            //Debug.WriteLine("WristLeft Y=" + WristLeft.Y + " Z=" + WristLeft.Z + " X=" + WristLeft.X + " W=" + WristLeft.W);
            //Debug.WriteLine("HandLeft Y=" + HandLeft.Y + " Z=" + HandLeft.Z + " X=" + HandLeft.X + " W=" + HandLeft.W);


            Quaternion composite = new Quaternion();

            return Matrix.CreateFromQuaternion(composite);
        }

        /// <summary>
        /// Simple function to Move the left fore arm "up" whatever up means. 
        /// 1st attempt was to move just the elbow and wrist, but I think the rigging on the mesh 
        /// requires the entire hand and finger joints too. 
        /// Going to try to remove the kinect skeleton from the mix and just move the avatar based on time 
        /// There is a normal for each skeleton joint that describes rotation
        /// Rotation is expressed as the vector (in world space) perpendicular to the attached bones in the joint hierarchy.
        ///  to determine the roll of the right elbow, the immediate parent joint in the hierarchy, the right shoulder, is used to determine the plane of the bone.
        /// </summary>
        /// <param name="bone">Nui Joint/bone orientation</param>
        /// <param name="skeleton">The Kinect skeleton.</param>
        /// <param name="bindRoot">The bind root matrix of the avatar mesh.</param>
        /// <param name="boneTransforms">The avatar mesh rotation matrices.</param>
        private Matrix processBone2(JointOrientation bone, Body skeleton, Matrix bindRoot, ref Matrix[] boneTransforms)
        {
            // #1 Start with Parent Joint to find this joint's new position 

            // use time every 10 seconds  to just move arm up and down up and down... 
            //bool armup = (Math.Round((float)(this.gameLoopCounter / 10) % 2) == 0);

            // If the joint has a parent, constrain the bone direction to be within the constraint cone
            JointType parentIdx = KinectHelper.ParentBoneJoint(bone.JointType);

            Joint parentJoint = new Joint();
            skeleton.Joints.TryGetValue(parentIdx, out parentJoint);

            Matrix parentMatrix = KinectHelper.JointOrientationToXNAMatrix(skeleton.JointOrientations[parentIdx].Orientation);
            Matrix targetJointMatrix = KinectHelper.JointOrientationToXNAMatrix(bone.Orientation);

            // Kinect = +Y along leg (down), +Z fwd, +X right in body coordinate system
            // Avatar = +X along leg (down), +Y fwd, +Z right
            Quaternion kinectRotation = KinectHelper.DecomposeMatRot(parentMatrix);  // XYZ
            Quaternion avatarRotation = new Quaternion(kinectRotation.Y, kinectRotation.Z, kinectRotation.X, kinectRotation.W); // transform from Kinect to avatar coordinate system
            Matrix newBoneRotation = Matrix.CreateFromQuaternion(avatarRotation);
            //// get this joint's parent bone from the avatar's bone effects LeftWrist=14 Left fore arm. 
            int meshJointId = 0;
            bool success = this.nuiJointToAvatarBoneIndex.TryGetValue(bone.JointType, out meshJointId);

            int meshJointParentId = 0;
            bool success2 = this.nuiJointToAvatarBoneIndex.TryGetValue(parentIdx, out meshJointParentId);

            //targetJointMatrix.Translation = boneTransforms[meshJointId].Translation;
            // Calculate the absolute/world equivalents of the hierarchical rotation
            Quaternion parentRotation = KinectHelper.QuaternionFromVector4(skeleton.JointOrientations[parentIdx].Orientation);
            Quaternion relativeRotation = KinectHelper.QuaternionFromVector4(bone.Orientation);
            relativeRotation.Normalize();
            // boneOrientationRelativeToParentQuat= parent
            Quaternion inverseRotation = Quaternion.Inverse(parentRotation);
            Quaternion slerpedInverseRotation = Quaternion.Slerp(Quaternion.Identity, inverseRotation, 1);
            Quaternion constrainedRotation = parentRotation * slerpedInverseRotation;
            // Create a new world rotation Quaternion.Inverse(leftShoulderOrientation) * leftElbowOrientation
            Quaternion worldRotation = (inverseRotation * relativeRotation);
            //Quaternion.Multiply(parentRotation, relativeRotation);

            Matrix.Transform(ref boneTransforms[meshJointId], ref relativeRotation, out newBoneRotation);
            return newBoneRotation;
        }

        /// <summary>
        /// Set the bone transform in the avatar mesh.
        /// </summary>
        /// <param name="bone">Nui Joint/bone orientation</param>
        /// <param name="skeleton">The Kinect skeleton.</param>
        /// <param name="bindRoot">The bind root matrix of the avatar mesh.</param>
        /// <param name="boneTransforms">The avatar mesh rotation matrices.</param>
        private void SetJointTransformation(JointOrientation bone, Body skeleton, Matrix bindRoot, ref Matrix[] boneTransforms)
        {
            Vector3 scaleSkeletonToAvatar = new Vector3(1f, 1f, 1f);
            Matrix invBindRoot = bindRoot;
            invBindRoot.Translation = Vector3.Zero;
            invBindRoot = Matrix.Invert(invBindRoot); 
            Debug.WriteIf(gameLoopCounter%20==0 , " " + bone.JointType + " = " + bone.Orientation.X  + " "+ bone.Orientation.Y  + " " + bone.Orientation.Z  + " " + bone.Orientation.W );
            // Always look at the skeleton root
            if (bone.JointType == JointType.SpineBase)
            {
                //processWholeBody(bone, skeleton, bindRoot, ref boneTransforms);
                // Unless in seated mode, the hip center is special - it is the root of the NuiSkeleton and describes the skeleton orientation in the world
                // (camera) coordinate system. All other bones/joint orientations in the hierarchy have hip center as one of their parents.
                // However, if in seated mode, the shoulder center then holds the skeleton orientation in the world (camera) coordinate system.
                //bindRoot.Translation = Vector3.Zero;
                //Matrix invBindRoot = Matrix.Invert(bindRoot);

                Matrix hipOrientation = KinectHelper.JointOrientationToXNAMatrix(bone.Orientation);
                
                // Here we create a rotation matrix for the hips from the inverse of the bind pose
                // for the pelvis rotation and the inverse of the bind pose for the root node (0) in the Dude model.
                // This multiplication effectively removes the initial 90 degree rotations set in the first two model nodes.
                Matrix pelvis = boneTransforms[1];
                pelvis.Translation = Vector3.Zero; // Ensure pure rotation as we explicitly set world translation from the Kinect camera below.
                Matrix invPelvis = Matrix.Invert(pelvis);

                Matrix combined = (invBindRoot * hipOrientation) * invPelvis;

                this.ReplaceBoneMatrix(JointType.SpineBase, combined, true, ref boneTransforms);
            }
            else if (bone.JointType == JointType.SpineShoulder)
            {
                // This contains an absolute rotation if we are in seated mode, or the hip center is not tracked, as the HipCenter will be identity
                if (this.chooser.SeatedMode || (this.Chooser.SeatedMode == false && skeleton.Joints[JointType.SpineBase].TrackingState == TrackingState.NotTracked))
                {


                    Matrix hipOrientation = KinectHelper.JointOrientationToXNAMatrix(bone.Orientation);

                    // We can use the same method as in HipCenter above to invert the root and pelvis bind pose,
                    // however, alternately we can also explicitly swap axes and adjust the rotations to get from
                    // the Kinect rotation to the model hip orientation, similar to what we do for the following joints/bones.

                    // Kinect = +X left, +Y up, +Z forward in body coordinate system
                    // Avatar = +Z left, +X up, +Y forward
                    Quaternion kinectRotation = KinectHelper.DecomposeMatRot(hipOrientation);    // XYZ
                    Quaternion avatarRotation = new Quaternion(kinectRotation.Y, kinectRotation.Z, kinectRotation.X, kinectRotation.W); // transform from Kinect to avatar coordinate system
                    Matrix combined = Matrix.CreateFromQuaternion(avatarRotation);

                    // Add a small adjustment rotation to manually correct for the rotation in the parent bind
                    // pose node in the model mesh - this can be found by looking in the FBX or in 3DSMax/Maya.
                    Matrix adjustment = Matrix.CreateRotationY(MathHelper.ToRadians(-90));
                    combined *= adjustment;
                    Matrix adjustment2 = Matrix.CreateRotationZ(MathHelper.ToRadians(-90));
                    combined *= adjustment2;

                    // Although not strictly correct, we apply this to the hip center, as all other bones are children of this joint.
                    // Application at the spine or shoulder center instead would require manually updating of the bone orientations below for the whole body to move when the shoulders twist or tilt.
                    this.ReplaceBoneMatrix(JointType.SpineBase, combined, true, ref boneTransforms);
                }
            }
            else if (bone.JointType == JointType.SpineShoulder)
            {
                Matrix tempMat = KinectHelper.JointOrientationToXNAMatrix(bone.Orientation);

                // The Dude appears to lean back too far compared to a real person, so here we adjust this lean.
                this.CorrectBackwardsLean(skeleton, ref tempMat);

                // Also add a small constant adjustment rotation to correct for the hip center to spine bone being at a rear-tilted angle in the Kinect skeleton.
                // The dude should now look more straight ahead when avateering
                Matrix adjustment = Matrix.CreateRotationX(MathHelper.ToRadians(20));  // 20 degree rotation around the local Kinect x axis for the spine bone.
                tempMat *= adjustment;

                // Kinect = +X left, +Y up, +Z forward in body coordinate system
                // Avatar = +Z left, +X up, +Y forward
                Quaternion kinectRotation = KinectHelper.DecomposeMatRot(tempMat);    // XYZ
                Quaternion avatarRotation = new Quaternion(kinectRotation.Y, kinectRotation.Z, kinectRotation.X, kinectRotation.W); // transform from Kinect to avatar coordinate system
                tempMat = Matrix.CreateFromQuaternion(avatarRotation);

                // Set the corresponding matrix in the avatar using the translation table we specified.
                // Note for the spine and shoulder center rotations, we could also try to spread the angle
                // over all the Avatar skeleton spine joints, causing a more curved back, rather than apply
                // it all to one joint, as we do here.
                this.ReplaceBoneMatrix(bone.JointType, tempMat, false, ref boneTransforms);
            }
            else if (bone.JointType == JointType.Head)
            {
                Matrix tempMat = KinectHelper.JointOrientationToXNAMatrix(bone.Orientation);

                // Add a small adjustment rotation to correct for the avatar skeleton head bones being defined pointing looking slightly down, not vertical.
                // The dude should now look more straight ahead when avateering
                Matrix adjustment = Matrix.CreateRotationX(MathHelper.ToRadians(-30));  // -30 degree rotation around the local Kinect x axis for the head bone.
                tempMat *= adjustment;

                // Kinect = +X left, +Y up, +Z forward in body coordinate system
                // Avatar = +Z left, +X up, +Y forward
                Quaternion kinectRotation = KinectHelper.DecomposeMatRot(tempMat);    // XYZ
                Quaternion avatarRotation = new Quaternion(kinectRotation.Y, kinectRotation.Z, kinectRotation.X, kinectRotation.W); // transform from Kinect to avatar coordinate system
                tempMat = Matrix.CreateFromQuaternion(avatarRotation);

                // Set the corresponding matrix in the avatar using the translation table we specified
                this.ReplaceBoneMatrix(bone.JointType, tempMat, false, ref boneTransforms);
            }
            else if (bone.JointType == JointType.ShoulderLeft || bone.JointType == JointType.ElbowLeft || bone.JointType == JointType.WristLeft || bone.JointType == JointType.HandLeft)
            {
                Matrix tempMat = this.processBone2(bone, skeleton, bindRoot, ref boneTransforms);

                this.ReplaceBoneMatrix(bone.JointType, tempMat, true, ref boneTransforms);
            }
            //else if (bone.JointType == JointType.HandLeft)
            //{
            //    Matrix tempMat = KinectHelper.JointOrientationToXNAMatrix(bone.Orientation);
            //    // Add a small adjustment rotation to correct for the avatar skeleton wist/hand bone.
            //    // The dude should now have the palm of his hands toward his body when arms are straight down
            //    Matrix adjustment = Matrix.CreateRotationY(MathHelper.ToRadians(-90));  // -90 degree rotation around the local Kinect y axis for the wrist-hand bone.
            //    tempMat *= adjustment;

            //    // Kinect = +Y along arm, +X down, +Z forward in body coordinate system
            //    // Avatar = +X along arm, +Y down, +Z backwards
            //    Quaternion kinectRotation = KinectHelper.DecomposeMatRot(tempMat);    // XYZ
            //    Quaternion avatarRotation = new Quaternion(kinectRotation.Y, kinectRotation.Z, -kinectRotation.X, kinectRotation.W);
            //    tempMat = Matrix.CreateFromQuaternion(avatarRotation);

            //    this.ReplaceBoneMatrix(bone.JointType, tempMat, true, ref boneTransforms);
            //}
            else if (bone.JointType == JointType.ElbowRight || bone.JointType == JointType.WristRight)
            {
                Matrix tempMat = KinectHelper.JointOrientationToXNAMatrix(bone.Orientation);

                if (bone.JointType == JointType.ElbowRight)
                {
                    // Add a small adjustment rotation to correct for the avatar skeleton shoulder/upper arm bones.
                    // The dude should now be able to have arms correctly down at his sides when avateering
                    Matrix adjustment = Matrix.CreateRotationZ(MathHelper.ToRadians(15));  // 15 degree rotation around the local Kinect  z axis for the upper arm bone.
                    tempMat *= adjustment;
                }

                // Kinect = +Y along arm, +X up, +Z forward in body coordinate system
                // Avatar = +X along arm, +Y back, +Z down
                Quaternion kinectRotation = KinectHelper.DecomposeMatRot(tempMat);    // XYZ
                Quaternion avatarRotation = new Quaternion(kinectRotation.Y, -kinectRotation.Z, -kinectRotation.X, kinectRotation.W); // transform from Kinect to avatar coordinate system
                tempMat = Matrix.CreateFromQuaternion(avatarRotation);

                this.ReplaceBoneMatrix(bone.JointType, tempMat, false, ref boneTransforms);
            }
            else if (bone.JointType == JointType.HandRight)
            {
                Matrix tempMat = KinectHelper.JointOrientationToXNAMatrix(bone.Orientation); //  KinectHelper.Matrix4ToXNAMatrix(bone.HierarchicalRotation.Matrix);

                // Add a small adjustment rotation to correct for the avatar skeleton wist/hand bone.
                // The dude should now have the palm of his hands toward his body when arms are straight down
                Matrix adjustment = Matrix.CreateRotationY(MathHelper.ToRadians(90));  // -90 degree rotation around the local Kinect y axis for the wrist-hand bone.
                tempMat *= adjustment;

                // Kinect = +Y along arm, +X up, +Z forward in body coordinate system
                // Avatar = +X along arm, +Y down, +Z forwards
                Quaternion kinectRotation = KinectHelper.DecomposeMatRot(tempMat);    // XYZ
                Quaternion avatarRotation = new Quaternion(kinectRotation.Y, -kinectRotation.X, kinectRotation.Z, kinectRotation.W); // transform from Kinect to avatar coordinate system
                tempMat = Matrix.CreateFromQuaternion(avatarRotation);

                this.ReplaceBoneMatrix(bone.JointType, tempMat, false, ref boneTransforms);
            }
            else if (bone.JointType == JointType.KneeLeft)
            {
                // Combine the two joint rotations from the hip and knee
                Matrix hipLeft = KinectHelper.JointOrientationToXNAMatrix(skeleton.JointOrientations[JointType.HipLeft].Orientation);
                   // KinectHelper.Matrix4ToXNAMatrix(skeleton.JointOrientations[JointType.HipLeft].HierarchicalRotation.Matrix);
                Matrix kneeLeft = KinectHelper.JointOrientationToXNAMatrix(bone.Orientation); // KinectHelper.Matrix4ToXNAMatrix(bone.HierarchicalRotation.Matrix);
                Matrix combined = kneeLeft * hipLeft;

                this.SetLegMatrix(bone.JointType, combined, ref boneTransforms);
            }
            else if (bone.JointType == JointType.AnkleLeft || bone.JointType == JointType.AnkleRight)
            {
                Matrix tempMat = KinectHelper.JointOrientationToXNAMatrix(bone.Orientation); //  KinectHelper.Matrix4ToXNAMatrix(bone.HierarchicalRotation.Matrix);
                this.SetLegMatrix(bone.JointType, tempMat, ref boneTransforms);
            }
            else if (bone.JointType == JointType.KneeRight)
            {
                // Combine the two joint rotations from the hip and knee
                Matrix hipRight = KinectHelper.JointOrientationToXNAMatrix(skeleton.JointOrientations[JointType.HipRight].Orientation); //  KinectHelper.Matrix4ToXNAMatrix(skeleton.JointOrientations[JointType.HipRight].HierarchicalRotation.Matrix);
                Matrix kneeRight = KinectHelper.JointOrientationToXNAMatrix(bone.Orientation); //  KinectHelper.Matrix4ToXNAMatrix(bone.HierarchicalRotation.Matrix);
                Matrix combined = kneeRight * hipRight;

                this.SetLegMatrix(bone.JointType, combined, ref boneTransforms);
            }
            else if (bone.JointType == JointType.FootLeft || bone.JointType == JointType.FootRight)
            {
                // Only set this if we actually have a good track on this and the parent
                if (skeleton.Joints[bone.JointType].TrackingState == TrackingState.Tracked )
                {
                    Matrix tempMat = KinectHelper.JointOrientationToXNAMatrix(bone.Orientation); //  KinectHelper.Matrix4ToXNAMatrix(bone.HierarchicalRotation.Matrix);

                    // Add a small adjustment rotation to correct for the avatar skeleton foot bones being defined pointing down at 45 degrees, not horizontal
                    Matrix adjustment = Matrix.CreateRotationX(MathHelper.ToRadians(-45));
                    tempMat *= adjustment;

                    // Kinect = +Y along foot (fwd), +Z up, +X right in body coordinate system
                    // Avatar = +X along foot (fwd), +Y up, +Z right
                    Quaternion kinectRotation = KinectHelper.DecomposeMatRot(tempMat); // XYZ
                    Quaternion avatarRotation = new Quaternion(kinectRotation.Y, kinectRotation.Z, kinectRotation.X, kinectRotation.W); // transform from Kinect to avatar coordinate system
                    tempMat = Matrix.CreateFromQuaternion(avatarRotation);
                    
                    this.ReplaceBoneMatrix(bone.JointType, tempMat, false, ref boneTransforms);
                }
            }            
        }

        /// <summary>
        /// Correct the spine rotation when leaning back to reduce lean.
        /// </summary>
        /// <param name="skeleton">The Kinect skeleton.</param>
        /// <param name="spineMat">The spine orientation.</param>
        private void CorrectBackwardsLean(Body skeleton, ref Matrix spineMat)
        {
            // KinectHelper.JointOrientationToXNAMatrix(bone.Orientation); // 
            Matrix hipOrientation = KinectHelper.JointOrientationToXNAMatrix(skeleton.JointOrientations[JointType.SpineBase].Orientation);

            Vector3 hipZ = new Vector3(hipOrientation.M31, hipOrientation.M32, hipOrientation.M33);   // Z (forward) vector
            Vector3 boneY = new Vector3(spineMat.M21, spineMat.M22, spineMat.M23);   // Y (up) vector

            hipZ *= -1;
            hipZ.Normalize();
            boneY.Normalize();

            // Dot product the hip center forward vector with our spine bone up vector.
            float cosAngle = Vector3.Dot(hipZ, boneY);

            // If it's negative (i.e. greater than 90), we are leaning back, so reduce this lean.
            if (cosAngle < 0 && this.leanAdjust)
            {
                float angle = (float)Math.Acos(cosAngle);
                float correction = (angle / 2) * -(cosAngle / 2);
                Matrix leanAdjustment = Matrix.CreateRotationX(correction);  // reduce the lean by up to half, scaled by how far back we are leaning
                spineMat *= leanAdjustment;
            }
        }

        /// <summary>
        /// Helper used for leg bones.
        /// </summary>
        /// <param name="joint">Nui Joint index</param>
        /// <param name="legRotation">Matrix containing a leg joint rotation.</param>
        /// <param name="boneTransforms">The avatar mesh rotation matrices.</param>
        private void SetLegMatrix(JointType joint, Matrix legRotation, ref Matrix[] boneTransforms)
        {
            // Kinect = +Y along leg (down), +Z fwd, +X right in body coordinate system
            // Avatar = +X along leg (down), +Y fwd, +Z right
            Quaternion kinectRotation = KinectHelper.DecomposeMatRot(legRotation);  // XYZ
            Quaternion avatarRotation = new Quaternion(kinectRotation.Y, kinectRotation.Z, kinectRotation.X, kinectRotation.W); // transform from Kinect to avatar coordinate system
            legRotation = Matrix.CreateFromQuaternion(avatarRotation);

            this.ReplaceBoneMatrix(joint, legRotation, false, ref boneTransforms);
        }

        /// <summary>
        /// Set the avatar root position in world coordinates.
        /// </summary>
        /// <param name="skeleton">The Kinect skeleton.</param>
        /// <param name="boneTransforms">The avatar mesh rotation matrices.</param>
        private void SetAvatarRootWorldPosition(Body skeleton, ref Matrix[] boneTransforms)
        {
            // Get XNA world position of skeleton.
            Matrix worldTransform = this.GetModelWorldTranslation(skeleton, this.chooser.SeatedMode); 

            // set root translation
            boneTransforms[0].Translation = worldTransform.Translation;
        }

        /// <summary>
        /// This function sets the mapping between the Nui Skeleton bones/joints and the Avatar bones/joints
        /// </summary>
        /// <param name="joint">Nui Joint index</param>
        /// <param name="boneMatrix">Matrix to set in joint/bone.</param>
        /// <param name="replaceTranslationInExistingBoneMatrix">set Boolean true to replace the translation in the original bone matrix with the one passed in boneMatrix (i.e. at root), false keeps the original (default).</param>
        /// <param name="boneTransforms">The avatar mesh rotation matrices.</param>
        private void ReplaceBoneMatrix(JointType joint, Matrix boneMatrix, bool replaceTranslationInExistingBoneMatrix, ref Matrix[] boneTransforms)
        {
            int meshJointId;
            bool success = this.nuiJointToAvatarBoneIndex.TryGetValue(joint, out meshJointId);
            Matrix existingBoneTransform = boneTransforms[meshJointId];
            if (success)
            {
                Vector3 offsetTranslation = boneTransforms[meshJointId].Translation;

                // just changing the position 
                boneTransforms[meshJointId].Translation = boneMatrix.Translation;
                //boneTransforms[meshJointId] = boneMatrix;

                if (replaceTranslationInExistingBoneMatrix == false)
                {
                    // overwrite any new boneMatrix translation with the original one
                    boneTransforms[meshJointId].Translation = offsetTranslation;   // re-set the translation
                }
            }
            else
            {
                Debug.WriteLineIf(gameLoopCounter % 10 == 0, "bone joine false = " + joint + " meshid=" + meshJointId);
            }
        }

        /// <summary>
        /// Helper used to get the world translation for the root.
        /// </summary>
        /// <param name="joints">Nui Joint collection.</param>
        /// <param name="seatedMode">Boolean true if seated mode.</param>
        /// <returns>Returns a Matrix containing the translation.</returns>
        private Matrix GetModelWorldTranslation(Body body, bool seatedMode)
        {
            Vector3 transVec = Vector3.Zero;

            if (seatedMode && body.Joints[JointType.SpineShoulder].TrackingState != TrackingState.NotTracked)
            {
                transVec = KinectHelper.SkeletonPointToVector3(body.Joints[JointType.SpineShoulder].Position);
            }
            else
            {
                if (body.Joints[JointType.SpineBase].TrackingState != TrackingState.NotTracked)
                {
                    transVec = KinectHelper.SkeletonPointToVector3(body.Joints[JointType.SpineBase].Position);
                }
                else if (body.Joints[JointType.SpineShoulder].TrackingState != TrackingState.NotTracked)
                {
                    // finally try shoulder center if this is tracked while hip center is not
                    transVec = KinectHelper.SkeletonPointToVector3(body.Joints[JointType.SpineShoulder].Position);
                }
            }

            if (this.fixAvatarSpineBaseDrawHeight)
            {
                transVec.Y = this.avatarHipCenterDrawHeight;
            }

            // Here we scale the translation, as the "Dude" avatar mesh is defined in centimeters, and the Kinect skeleton joint positions in meters.
            return Matrix.CreateTranslation(transVec * SkeletonTranslationScaleFactor);
        }

        /// <summary>
        /// Sets the Avatar in a seated posture - useful for seated mode.
        /// </summary>
        /// <param name="boneTransforms">The relative bone transforms of the avatar mesh.</param>
        private void SetSeatedPosture(ref Matrix[] boneTransforms)
        {
            // In the Kinect coordinate system, we first rotate from the local avatar 
            // root orientation with +Y up to +Y down for the leg bones (180 around Z)
            // then pull the knees up for a seated posture.
            Matrix rot180 = Matrix.CreateRotationZ(MathHelper.ToRadians(180));
            Matrix rot90 = Matrix.CreateRotationX(MathHelper.ToRadians(90));
            Matrix rotMinus90 = Matrix.CreateRotationX(MathHelper.ToRadians(-90));
            Matrix combinedHipRotation = rot90 * rot180;

            this.SetLegMatrix(JointType.KneeLeft, combinedHipRotation, ref boneTransforms);
            this.SetLegMatrix(JointType.KneeRight, combinedHipRotation, ref boneTransforms);
            this.SetLegMatrix(JointType.AnkleLeft, rotMinus90, ref boneTransforms);
            this.SetLegMatrix(JointType.AnkleRight, rotMinus90, ref boneTransforms);
        }

        #endregion

        #region Helpers

        /// <summary>
        /// This method ensures that we can render to the back buffer without
        /// losing the data we already had in our previous back buffer.  This
        /// is necessary for the SkeletonStreamRenderer.
        /// </summary>
        /// <param name="sender">The sending object.</param>
        /// <param name="e">The event args.</param>
        private void GraphicsDevicePreparingDeviceSettings(object sender, PreparingDeviceSettingsEventArgs e)
        {
            // This is necessary because we are rendering to back buffer/render targets and we need to preserve the data
            e.GraphicsDeviceInformation.PresentationParameters.RenderTargetUsage = RenderTargetUsage.PreserveContents;
        }

        /// <summary>
        /// This method maps a SkeletonPoint to the depth frame. normalized to maxsize of the window
        /// </summary>
        /// <param name="point">The SkeletonPoint to map.</param>
        /// <returns>A Vector2 of the location on the depth frame.</returns>
        private Vector2 SkeletonToDepthMap(CameraSpacePoint depthPt)
        {
            // This is used to map a skeleton point to the depth image location
            if (null == this.chooser || null == this.Chooser.Sensor || true != this.Chooser.Sensor.IsOpen )
            {
                return Vector2.Zero;
            }

            DepthSpacePoint depthSpacePoint = this.coordinateMapper.MapCameraPointToDepthSpace(depthPt);// convert a skeleton joint camera point space joints[jointType].Position

            //this.chooser.Sensor.CoordinateMapper.MapCameraPointsToDepthSpace(skeletonStream., depthPoints);// MapSkeletonPointToDepthPoint(point, this.chooser.Sensor.ColorFrameSource);

            // scale to current depth image display size and add any position offset
            float x = depthSpacePoint.X / this.depthStream.depthWidth * this.skeletonStream.Size.X;//(depthPt.X * this.skeletonStream.Size.X) / depthSpacePoint.X;
            float y = depthSpacePoint.Y / this.depthStream.depthHeight * this.skeletonStream.Size.Y;//(depthPt.Y * this.skeletonStream.Size.Y) / depthSpacePoint.Y;

            // removed the extra x & y plus on the return, just return the coordinatemapper 
            return new Vector2(590+x, 85+y);
        }

        #endregion
    }
}